/**
 * 
 */
package mapmaker.map;

import java.awt.image.BufferedImage;
import java.awt.image.WritableRaster;
import java.io.File;
import java.io.IOException;
import java.util.ArrayList;

import javax.imageio.ImageIO;

import mapmaker.config.Config;
import mapmaker.pose.Position;
import mapmaker.utils.Utilities;


public class EnvironmentMap {

	private static double WIDTH;
	private static double HEIGHT;
	private static int CELL_MAX_WIDTH;
	private static int CELL_MAX_HEIGHT;
	private static double _DX;
	private static double _DY;
	private int _min_unseen_depth;
	private MapCell[][] _map;

	public EnvironmentMap(Position botLeft, Position topRight){
		_min_unseen_depth = 0;
		double distanceX = Math.abs(botLeft.getX() - topRight.getX());
		double distanceY = Math.abs(topRight.getY() - botLeft.getY());
		WIDTH = distanceX;
		HEIGHT = distanceY;
		int nbRow = (int) Math.ceil(distanceY / Config.MAX_LASER_RANGE);
		int nbCol = (int) Math.ceil(distanceX / Config.MAX_LASER_RANGE);
		_DX = distanceY / nbRow;
		_DY = distanceX / nbCol;
		CELL_MAX_WIDTH = Utilities.nextPowerOf2((int) WIDTH / nbCol);
		CELL_MAX_HEIGHT = Utilities.nextPowerOf2((int) HEIGHT / nbRow);
		Config.DEPTH_MAX = (int) (Math.log10(CELL_MAX_HEIGHT) / Math.log10(2));
		_map = new MapCell[nbRow][nbCol];
		for(int i = 0; i < nbRow; i++){
			for(int j = 0; j < nbCol; j++){
				Position cellBotLeft = new Position(botLeft.getX() + j * _DX, botLeft.getY() + i * _DY, 0);
				Position cellTopRight = new Position(botLeft.getX() + j * _DX + _DX, botLeft.getY() + i * _DY + _DY, 0);
				_map[i][j] = new MapCell(cellBotLeft, cellTopRight, i, j);
			}
		}
	}

	public int getMinUnSeenDepth() {
		return _min_unseen_depth;
	}

	/**
	 * Get the corresponding cell in map representation from a position 
	 * @param p a position
	 * @return the corresponding cell
	 */
	public MapCell getCellAt(Position p){
		MapCell parent;
		if((parent = getBiggestCellAt(p, _map)) != null){
			while(parent.hasChildren()){
				parent = getBiggestCellAt(p, parent.getChildren());
			}
			return parent;
		}else{
			return null;
		}
	}

	/**
	 * Get a cell at (row,col) in the map representation
	 * @param row
	 * @param col
	 * @return
	 */
	public MapCell getCellAt(int row, int col){
		if(row >= 0 && col >= 0 && row < _map.length && col < _map[0].length) {
			return _map[row][col];			
		} else {
			return null;
		}
	}

	/**
	 * @return the number of row in the map representation
	 */
	public int getNbRow() {
		return _map.length;
	}

	/**
	 * @return the number of cols in the map reprensentation
	 */
	public int getNbCol() {
		return _map[0].length;
	}

	/**
	 * Get the biggest cell in the map reprensentation corresponding to a position
	 * @param p a position
	 * @param map
	 * @return
	 */
	public MapCell getBiggestCellAt(Position p, MapCell[][] map){
		for(int i = 0; i < map.length; i++){
			for(int j = 0; j < map[i].length;j++){
				if(map[i][j].positionIsIn(p)){
					return map[i][j];
				}
			}
		}
		return null;
	}

	/**
	 * Generate map image 
	 * @param fileName output file name
	 */
	public void generateMapImage(String fileName){
		int realWidth = Utilities.nextPowerOf2((int) Math.ceil(WIDTH));
		int readHeight = Utilities.nextPowerOf2((int) Math.ceil(HEIGHT));
		BufferedImage img = new BufferedImage(realWidth, readHeight, BufferedImage.TYPE_INT_RGB);
		WritableRaster raster = img.getRaster();
		int[] white = {0xff, 0xff, 0xff};
		for(int i = 0; i < readHeight; i++) {
			for(int j = 0; j < realWidth; j++) {
				raster.setPixel(j, i, white);
			}
		}
		for(int i = 0; i < _map.length; i++){
			for(int j = 0; j < _map[i].length; j++){
				printMapCell(_map[(_map.length -1) - i][j], j * CELL_MAX_WIDTH,  i * CELL_MAX_HEIGHT, raster);
			}
		}
		try {
			ImageIO.write(img, "gif", new File(fileName));
		} catch (IOException e) {
			e.printStackTrace();
		}
	}

	public MapCell[][] getMap(){
		return _map;

	}

	public void printCell(MapCell c, int x, int y,  WritableRaster raster){
		int realWidth = Utilities.nextPowerOf2((int) Math.ceil(WIDTH));
		int readHeight = Utilities.nextPowerOf2((int) Math.ceil(HEIGHT));
		int iMax = (int) (CELL_MAX_HEIGHT / Math.pow(2, c.getDepth()));
		int jMax = (int) (CELL_MAX_WIDTH / Math.pow(2, c.getDepth()));
		for(int i = 0; i < iMax; i++){
			for(int j = 0; j < jMax; j++){
				if(x+j < realWidth && y+i < readHeight){
					raster.setPixel(x + j, y + i, getColorForCell(c));
				}
			}
		}
	}

	public void printMapCell(MapCell c, int x, int y , WritableRaster raster){
		if(!c.hasChildren()){
			printCell(c, x, y, raster);
		}else{
			MapCell[][] children = c.getChildren();
			for(int i = 0; i < children.length; i++){
				for(int j = 0; j < children[i].length; j++){
					int offsetX = (int) (j * CELL_MAX_WIDTH / Math.pow(2 ,children[i][j].getDepth()));
					int offsetY = (int) (i * CELL_MAX_HEIGHT / Math.pow(2, children[i][j].getDepth()));
					printMapCell(children[children.length - 1 - i][j], x + offsetX, y + offsetY, raster);
				}
			}
		}
	}

	private int[] getColorForCell(MapCell c){
		return Config.GREY_COLORS[c.getWeight()];
	}


	/**
	 * Get neighbourhood of a cell
	 * @param current cell from which the expand will start
	 * @return
	 */
	public ArrayList<MapCell> expand(MapCell current) {
		ArrayList<MapCell> next = new ArrayList<MapCell>();
		ArrayList<MapCell> list = expandAux(current, 0, true); // BOTT
		if(list != null) {
			next.addAll(list);
		}
		list = expandAux(current, 0, false); // LEFT
		if(list != null) {
			next.addAll(list);
		}
		list = expandAux(current, 1, false); // RIGHT
		if(list != null) {
			next.addAll(list);
		}
		list = expandAux(current, 1, true); // TOP
		if(list != null) {
			next.addAll(list);
		}
		return next;
	}

	private ArrayList<MapCell> expandAux(MapCell current, int index, boolean vert) {
		MapCell cell = getClosestUp(current, index, vert);
		if(cell == null) {
			MapCell tmp = current;
			if(current.getDepth() > 0) {
				tmp = getBiggestCellAt(current.getMiddleCell(), _map);
			}
			cell = vert ? getCellAt(tmp.getIndexRow() - ((index == 1) ? -1 : 1), tmp.getIndexCol()) : 
						  getCellAt(tmp.getIndexRow(), tmp.getIndexCol() - ((index == 1) ? -1 : 1));
			if(cell == null) {
				return null;
			}
		} else {
			MapCell[][] children = cell.getChildren();
			MapCell mommy = getParentAtDepth(current, cell.getDepth() + 1);
			cell = vert ? children[index][mommy.getIndexCol()] : children[mommy.getIndexRow()][index];
		}
		
		while(cell.getDepth() < current.getDepth() && cell.hasChildren()) {
			MapCell[][] children = cell.getChildren();
			cell = vert ? children[index == 0 ? 1 : 0][current.getIndexCol()] : children[current.getIndexRow()][index == 0 ? 1 : 0];
		}

		ArrayList<MapCell> ret = null;
		if(!cell.hasChildren() && !cell.isUnreachable() && cell.getWeight() < 4) {
			ret = new ArrayList<MapCell>();
			ret.add(cell);
		} else {
			ret = vert ? getClosestChild(cell, index == 0 ? 1 : 0, 0, index == 0 ? 1 : 0, 1) :
						 getClosestChild(cell, 0, index == 0 ? 1: 0, 1, index == 0 ? 1: 0);
		}
		return ret;
	}

	private MapCell getParentAtDepth(MapCell current, int i) {
		while(current != null && current.getDepth() > i) {
			current = current.getParent();
		}
		return current;
	}

	private MapCell getClosestUp(MapCell cell, int index, boolean vert) {
		MapCell parent = cell.getParent();
		if(parent == null) {
			return parent;
		} else if((vert && cell.getIndexRow() != index) || (!vert && cell.getIndexCol() != index)) {
			return parent;
		} else {
			while((vert ? cell.getIndexRow() == index : cell.getIndexCol() == index) && parent != null) {
				cell = parent;
				parent = cell.getParent();
			}
			return parent;
		}
	}

	private ArrayList<MapCell> getClosestChild(MapCell cell, int i1, int j1, int i2, int j2) {
		if(cell.hasChildren()) {
			MapCell[][] children = cell.getChildren();
			ArrayList<MapCell> ret = getClosestChild(children[i1][j1], i1, j1, i2, j2);
			ArrayList<MapCell> ret2 = getClosestChild(children[i2][j2], i1, j1, i2, j2);
			if(ret != null) {
				if(ret2 != null) {
					ret.addAll(ret2);
				}
				return ret;
			} else {
				return ret2;
			}
		} else {
			if(!cell.isUnreachable() && cell.getWeight() < 4) {
				ArrayList<MapCell> ret = new ArrayList<MapCell>();
				ret.add(cell);
				return ret;
			}
		}
		return null;
	}

	/**
	 * Increment the minimum depth of unseen cells
	 */
	public void setMinUnSeenDepth() {
		_min_unseen_depth++;
	}

	/**
	 * Get the size of a cell in meters
	 * @param goal the cell to evaluate
	 * @return
	 */
	public double getSize(MapCell goal) {
		return Math.max(_DX / Math.pow(2, goal.getDepth()), _DY / Math.pow(2,  goal.getDepth()));
	}

	/**
	 * Get the minimum size in meters for a cell
	 * @return
	 */
	public double getMinSize() {
		return Math.max(_DX / Math.pow(2, Config.DEPTH_MAX), _DY / Math.pow(2, Config.DEPTH_MAX));
	}
}
